import com.cyberbotics.webots.controller.Camera;
import com.cyberbotics.webots.controller.DifferentialWheels;

/**
 * @author Benjamin 
 * Hier wird der Ball an die Wand gefahren
 */
public class PushBallToWallAgent extends AbstractAgent {

	private Camera camera;
	private Speed curSpeed;

	public PushBallToWallAgent() {
		curSpeed = new Speed(MIN_SPEED, MIN_SPEED);
		camera = getCamera("camera");
		camera.enable(10);
	}

	/**
	 * prüft ob ein rotes pixel vorhanden ist rot ist ein Pixel nur dann wenn
	 * keine grünen und blau vorhanden ist.
	 * 
	 * @return wenn der Pixel rot ist
	 */
	public boolean isRedPixel(int green, int blue, int red) {
		return green == 0 && blue == 0 && red > 0;
	}

	@Override
	public boolean takeControl() {
		int[] image = camera.getImage();
		int colorOfGreenPixel;
		int colorOfBluePixel;
		int colorOfRedPixel;
		int positionOfBall = 0;

		// durch die Pixel durch iterieren
		for (int curPixel = 0; curPixel < image.length; curPixel++) {

			colorOfGreenPixel = Camera.pixelGetGreen(image[curPixel]);
			colorOfBluePixel = Camera.pixelGetBlue(image[curPixel]);
			colorOfRedPixel = Camera.pixelGetRed(image[curPixel]);

			if (isRedPixel(colorOfGreenPixel, colorOfBluePixel, colorOfRedPixel)) {
				int prevPixel = curPixel;

				while (isRedPixel(colorOfGreenPixel, colorOfBluePixel, colorOfRedPixel)
						&& prevPixel % camera.getWidth() < camera.getWidth() - 1) {
					prevPixel++;
					colorOfGreenPixel = Camera.pixelGetGreen(image[prevPixel]);
					colorOfBluePixel = Camera.pixelGetBlue(image[prevPixel]);
					colorOfRedPixel = Camera.pixelGetRed(image[curPixel]);
				}

				// Mitte des Balles
				positionOfBall = ((prevPixel % camera.getWidth()) + (curPixel % camera
						.getWidth())) / 2;

				// links fahren wenn ball links ist
				if (positionOfBall < camera.getWidth() / 2 - 1) {
					curSpeed.setLeft(MIN_SPEED);
					curSpeed.setRight(MAX_SPEED);
				}
				// rechts fahren falls ball rechts ist
				else if (positionOfBall > camera.getWidth() / 2 + 1) {
					curSpeed.setLeft(MAX_SPEED);
					curSpeed.setRight(MIN_SPEED);
				}
				// vorw�rts fahren wenn ball mittig vorne
				else {
					curSpeed.setLeft(MAX_SPEED);
					curSpeed.setRight(MAX_SPEED);
				}
				return true;
			}
		}
		return false;
	}

	@Override
	public void action() {
		if (step(TIME_STEP) != -1) {
			driveWithSpeed(curSpeed);
		}
	}

}